#include <cmath>
#include <iostream>
#include <queue>
#include <vector>
#include <list>
#include <algorithm>
#include "fmm.h"
#include "backtracking.h"
#include "sknn.h"


double interpolate_point_value (Mesh & mesh, Mesh::Point & point, const Mesh::FaceHandle & face_handle)
{
	Backtracking<double>::POINT vertex [3], flip_v[3];
	int count = 0;
	double value[3];
	for (Mesh::FaceVertexIter fv_iter = mesh.fv_iter(face_handle); fv_iter; ++fv_iter)
	{
		vertex[count] = mesh.point(fv_iter.handle());
		value[count] = mesh.data(fv_iter.handle()).get_distance();
//		std::cout<<"Value : "<<value[count]<<std::endl;
		count++;
	}
	Backtracking<double>::MATRIX rotation;
	Backtracking<double>::VECTOR translation; 
	Backtracking<double>::map_threepoint_to_plane (vertex, flip_v, rotation, translation);
	
	Backtracking<double>::VECTOR coeff;
	Backtracking<double>::interpolation3 (flip_v, value, coeff);
	
	Backtracking<double>::POINT plane_point ;
	Backtracking<double>::convert_A2B (point, rotation, translation, plane_point);

	return (coeff[0] + coeff[1] * plane_point[0] + coeff[2] * plane_point[1]); 
}



bool compare_distance(const Point_info & it_pre, const Point_info & it_succ)
{
	return (it_pre.iter->second < it_succ.iter->second); 
}

int sknn(Mesh & mesh, Mesh::Point & start_point, Mesh::FaceHandle  start_face_handle, int num_neighbors, std::vector<Point_info> & neighbors )
{
	//Verify the start_point is on the face.
	//End verification.
	//this mesh should have some point attached to the face 
	face_point_fmm(mesh, start_face_handle.idx(), start_point);
	//Compute all the distance of every points
	/*
	for (Mesh::VertexIter v_it=mesh.vertices_begin(); v_it!=mesh.vertices_end(); ++v_it) 
	{
		std::cout<<"Value of V "<<mesh.data(v_it.handle()).get_distance()<<std::endl;
	}
	*/
	
	std::vector <Point_info> iterator_vector;
	for (Mesh::FaceIter f_iter = mesh.faces_begin(); f_iter != mesh.faces_end(); ++f_iter)
	{
		std::list<std::pair <Mesh::Point, double> >::iterator it_begin = mesh.data(f_iter.handle()).point_set.begin();
		std::list<std::pair <Mesh::Point, double> >::iterator it_end = mesh.data(f_iter.handle()).point_set.end();
		
		for (std::list<std::pair <Mesh::Point, double> >::iterator it = it_begin; it!= it_end; ++it )
		{
			it->second = interpolate_point_value (mesh, it->first, f_iter.handle());
			Point_info pInfo;
			pInfo.iter = it;
			pInfo.fh = f_iter.handle();
			iterator_vector.push_back(pInfo);
		}
	}

	std::sort (iterator_vector.begin(), iterator_vector.end(), compare_distance);
	/*
	for (int i = 0; i < iterator_vector.size(); ++i)
	{
		std::cout<< iterator_vector[i].iter->first<<" | ";
		std::cout<< iterator_vector[i].iter->second<<std::endl;
	}
	*/

	for (int i = 0 ; i < num_neighbors ; ++i)
	{
		neighbors.push_back(iterator_vector[i]);
	}

	//After compute the sknn. We need to finalize the value of all the points.
	
	return 0;
}
int marching_sknn (Mesh & mesh, std::vector<Mesh::VertexHandle> & front)
{
	return 0;
}




